#ifndef GMM__H
#define GMM__H

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigenvalues> 
#include <random>
#include <pcl/common/transforms.h>
#include <pcl/common/angles.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
#include <ros/ros.h>


namespace gvm
{


struct normal_random_variable
{
    normal_random_variable(Eigen::MatrixXd const& covar)
        : normal_random_variable(Eigen::VectorXd::Zero(covar.rows()), covar)
    {}

    normal_random_variable(Eigen::VectorXd const& mean, Eigen::MatrixXd const& covar)
        : mean(mean)
    {	
		
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
		int size = eigenSolver.eigenvalues().size();
		Eigen::VectorXd eigenvector = eigenSolver.eigenvalues();
		for (int i = 0; i < size; i++)
		{
			if (eigenvector(i) < 0)
			{
				std::cout << "==========eigen of covar is negative: "<< eigenvector(i)<< "============" << std::endl;
				eigenvector(i) = -eigenvector(i) / 10.0 ; // TODO
			}
		}
		transform = eigenSolver.eigenvectors() * eigenvector.cwiseSqrt().asDiagonal(); // TODO
		// std::cout << "==========covar============" << std::endl;
		// std::cout << covar << std::endl;
		// std::cout << "========== eigenSolver.eigenvectors()============" << std::endl;
		// std::cout <<  eigenSolver.eigenvectors() << std::endl;
		// std::cout << "==========eigenSolver.eigenvalues()============" << std::endl;
		// std::cout << eigenSolver.eigenvalues() << std::endl;
		// std::cout << "==========TRANSFORM============" << std::endl;
		// std::cout << transform << std::endl;
	}

	Eigen::VectorXd mean;
    Eigen::MatrixXd transform;

    Eigen::VectorXd operator()() const
    {
        static std::mt19937 gen{ std::random_device{}() };
        static std::normal_distribution<> dist;

        return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](double x) { return dist(gen); });
    }
};



class GMM
{
	public:
		GMM(){}
		~GMM(){}
		GMM(std::vector<double> pi_vector,std::vector<Eigen::Vector3d> means, std::vector<Eigen::Matrix3d> var,int n_component):
			pi_vector_(pi_vector),
			means_(means),
			var_(var),
			n_component_(n_component)
		{
			for(int i=0;i<n_component_;i++)
			{
				normal_random_variable random_variable(means[i].cast<double>(),var[i].cast<double>());
				randomGenerator_.push_back(random_variable);
			}
		}

		std::vector<double> pi_vector_;
		std::vector<Eigen::Vector3d> means_;
		std::vector<Eigen::Matrix3d> var_;
		std::vector<normal_random_variable> randomGenerator_;
		int n_component_;
		int agent_id{0};
		int color_id{37};
		double n_sigma{1.6};

	public:

		/**
		 * @brief Get the Mahalanobis Distance object
		 * @param point 
		 * @param means 
		 * @param var_inver 必须是协方差矩阵的逆
		 * @return double 
		 */
		inline double getMahalanobisDistance(Eigen::Vector3d point,Eigen::Vector3d means,Eigen::Matrix3d var_inver)
		{
			double distance=(point-means).transpose()*var_inver*(point-means);
			return distance;
		}

		void init(std::vector<double> pi_vector,std::vector<Eigen::Vector3d> means, std::vector<Eigen::Matrix3d> var,int n_component)
		{
			means_.clear();
			var_.clear();
			pi_vector_.clear();
			randomGenerator_.clear();

			pi_vector_ = pi_vector;
			means_=means;
			var_=var;
			n_component_=n_component;
			
			for(int i=0;i<n_component_;i++)
			{
				normal_random_variable random_variable(means[i].cast<double>(),var[i].cast<double>());
				randomGenerator_.push_back(random_variable);
			}
		}

		void setAgentId(int agentSetId, int colorSetId, double nsigma)
		{
			agent_id = agentSetId;
			color_id = colorSetId;
			n_sigma = nsigma;
		}

		double getGaussianProbability(Eigen::VectorXd x,Eigen::VectorXd u,Eigen::MatrixXd var)
		{
			double PI=3.1415926;
			double tmp=1.0/pow(2*PI,1.5)/pow(var.determinant(),0.5)*exp(-0.5*(x-u).transpose()*var.inverse()*(x-u));
			// ROS_INFO_STREAM("GAUSSIAN u:"<<u<<" var: "<<var<<"var determinant: "<<var.determinant());
			// ROS_INFO("GAUSSIAN: %f",tmp);
			return tmp;
		}

		void resample(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
		{
			for (int i = 0; i < n_component_; i++)
			{
				Eigen::Matrix3d var_inver = var_[i].inverse();
				int count = 0;
				int clusterPoint_num = (int)pi_vector_[i];
				// for (int j = 0; j < clusterPoint_num; j++)
				while(count < clusterPoint_num)
				{
					Eigen::Vector3d tmp_point = randomGenerator_[i]();
					if (getMahalanobisDistance(tmp_point, means_[i], var_inver) > pow(n_sigma, 2))
						continue;
					pcl::PointXYZ point = {(float)tmp_point(0), (float)tmp_point(1), (float)tmp_point(2) };
					cloud->push_back(point);
					count++;
					// if(agent_id == 0){
					// 	std::cout << "x:" << tmp_point[0] << "y:" << tmp_point[1] << "z:" << tmp_point[2] << std::endl;
					// }
				}
				// std::cout <<"count:" << count << "clusterPoint_num:" << clusterPoint_num << std::endl;

				// if(agent_id == 0){
				// 	std::cout << "=================================================================" << std::endl;
				// 	std::cout << "x:" << means_[i](0) << "y:" << means_[i](1) << "z:" << means_[i](2) << "count:" << count << "clusterPoint_num:" << clusterPoint_num << std::endl;
				// }
				// std::cout << "cloud->points.size()"<<cloud->points.size()<< std::endl;
				// std::cout << "x:" << means_[i](0) << "y:" << means_[i](1) << "z:" << means_[i](2) << "count:" << count << "clusterPoint_num:" << clusterPoint_num << std::endl;
			}
		}
};


}

#endif
